package pckg;

import pckg.realRobot.RealRobot;
import pckg.realRobot.uC.UC.Side;

public class Decomposer {
	
	private RealRobot robot;
	private Surf surf;
	private State idealStartState;
	private double deltaT = 0.1;
	
	public void evaluate(State start, RobotCommand cmd){
		idealStartState = new State(start);
		idealStartState.fi = 0;
		idealStartState.vL = cmd.getRealV(Side.LEFT);
		idealStartState.vR = cmd.getRealV(Side.RIGHT);
		
		long startT = System.nanoTime();
		
		double vL = cmd.getRealV(Side.LEFT);
		double vR = cmd.getRealV(Side.RIGHT);
		double t = (cmd.getRealTime()/RealLifeProperties.m2mm);
		
		RobotCommand tmpCmd = new RobotCommand(cmd);
		State prevState = new State(start);
		State lastIdeal = idealStartState;
		
		int prevSpL = cmd.getPrevSpeed(Side.LEFT);
		int prevSpR = cmd.getPrevSpeed(Side.RIGHT);
		
		double actT = 0;
		
		while (actT + 2*deltaT < t){
			
			tmpCmd.newCommand(	vL,
								vR, 
								(int)Math.round(deltaT*1000), 
								prevSpL, 
								prevSpR);
			
			tmpCmd = surf.evaluate(	prevState, 
									tmpCmd, 
									lastIdeal);
			
			prevState = robot.runCmd(prevState, tmpCmd, null, false);	
			
			prevSpL = RealLifeProperties.vToSpeed(robot.getV(Side.LEFT));
			prevSpR = RealLifeProperties.vToSpeed(robot.getV(Side.RIGHT));
			
			actT += deltaT;
			
			RealRobot.idealMove(lastIdeal, 
								(long)(deltaT*RealLifeProperties.s2ns));
		}
		
		double remaining = t-actT;
		
		for (int i = 0 ; i < 2 ; i++){
			tmpCmd.newCommand(	vL,
								vR, 
								(int)Math.round(remaining*1000.0/2.0), 
								prevSpL, 
								prevSpR);
						
			tmpCmd = surf.evaluate(	prevState, 
									tmpCmd, 
									lastIdeal);
			
			prevState = robot.runCmd(prevState, tmpCmd, null, false);	
			
			prevSpL = RealLifeProperties.vToSpeed(robot.getV(Side.LEFT));
			prevSpR = RealLifeProperties.vToSpeed(robot.getV(Side.RIGHT));
			
			vL = robot.getV(Side.LEFT);
			vR = robot.getV(Side.RIGHT);
			
			actT += remaining/2.0;
			
			RealRobot.idealMove(lastIdeal, 
								(long)(remaining*RealLifeProperties.s2ns/2.0));
		}
		
		long endT = System.nanoTime();
		
		System.out.println("Decomposer done in: " + (endT-startT)/RealLifeProperties.s2ns + "s.");
		
	}
	
	public RealRobot getRobot(){
		return robot;
	}
	
	public void resetRobot(){
		robot.reset();
	}
	
	public Decomposer(){
		robot = new RealRobot();
		surf = new Surf();
	}
	
}
